#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <tf2/LinearMath/Transform.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <mutex>

class CalibOdomTransformer : public rclcpp::Node
{
public:
    CalibOdomTransformer()
        : Node("calib_odom_transformer")
    {
        // 可选：创建callback group，分离两个回调到不同线程（可省略）
        extrinsics_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
        odom_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

        // 订阅外参话题 extrinsics
        rclcpp::SubscriptionOptions extrinsics_opt;
        extrinsics_opt.callback_group = extrinsics_group_;
        extrinsics_sub_ = this->create_subscription<geometry_msgs::msg::TransformStamped>(
            "extrinsics", 10,
            std::bind(&CalibOdomTransformer::extrinsics_callback, this, std::placeholders::_1),
            extrinsics_opt);

        // 订阅 GPS 里程计数据
        rclcpp::SubscriptionOptions odom_opt;
        odom_opt.callback_group = odom_group_;
        gps_odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
            "Odometry/transform", 50,
            std::bind(&CalibOdomTransformer::gps_odom_callback, this, std::placeholders::_1),
            odom_opt);

        // 发布转换后的 odom
        odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("odom_transformed", 10);

        RCLCPP_INFO(this->get_logger(), "标定对齐定位转换节点已启动");
    }

private:
    geometry_msgs::msg::TransformStamped::SharedPtr latest_extrinsics_;
    std::mutex extrinsics_mutex_; // 保护外参缓存多线程安全

    rclcpp::Subscription<geometry_msgs::msg::TransformStamped>::SharedPtr extrinsics_sub_;
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr gps_odom_sub_;
    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;

    rclcpp::CallbackGroup::SharedPtr extrinsics_group_;
    rclcpp::CallbackGroup::SharedPtr odom_group_;

    // 外参回调，缓存最新的标定
    void extrinsics_callback(const geometry_msgs::msg::TransformStamped::SharedPtr msg)
    {
        // 加锁保护
        std::lock_guard<std::mutex> lock(extrinsics_mutex_);
        latest_extrinsics_ = msg;
        RCLCPP_INFO(this->get_logger(), "收到新的外参标定数据。");
    }

    // GPS 里程计回调，进行变换并发布
    void gps_odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
    {
        geometry_msgs::msg::TransformStamped::SharedPtr extrinsics_copy;
        {
            std::lock_guard<std::mutex> lock(extrinsics_mutex_);
            if (!latest_extrinsics_)
            {
                RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "尚未收到外参数据，无法进行变换。");
                return;
            }
            extrinsics_copy = latest_extrinsics_;
        }

        nav_msgs::msg::Odometry odom_out;
        transformOdom(*msg, odom_out, *extrinsics_copy);
        odom_pub_->publish(odom_out);
        RCLCPP_INFO(this->get_logger(), "已发布转换后的里程计数据。");
    }
    // 里程计变换函数
    void transformOdom(
        const nav_msgs::msg::Odometry &gps_odom,
        nav_msgs::msg::Odometry &odom_out,
        const geometry_msgs::msg::TransformStamped &tfs_rcv)
    {
        // 1. 将 TransformStamped 转换为 tf2::Transform
        tf2::Transform tf2_xform;
        tf2::fromMsg(tfs_rcv.transform, tf2_xform);

        // 2. 将 gps_odom 的位姿转换为 tf2::Transform
        tf2::Transform tf2_pose;
        tf2::fromMsg(gps_odom.pose.pose, tf2_pose);

        // 3. 应用变换（将外参作用到GPS里程计位姿上）
        tf2::Transform tf2_pose_out = tf2_xform * tf2_pose;

        // 4. 填充 odom_out 的 header 和 frame 信息
        odom_out.header.stamp = tfs_rcv.header.stamp;
        odom_out.header.frame_id = "camera_init";
        odom_out.child_frame_id = gps_odom.child_frame_id;

        // 5. 构造 PoseWithCovariance（位姿和协方差）
        geometry_msgs::msg::PoseWithCovariance p_cov;
        // 设置位置
        geometry_msgs::msg::Point pt;
        pt.x = tf2_pose_out.getOrigin().x();
        pt.y = tf2_pose_out.getOrigin().y();
        pt.z = tf2_pose_out.getOrigin().z();
        p_cov.pose.position = pt;
        // 设置姿态（四元数）
        p_cov.pose.orientation = tf2::toMsg(tf2_pose_out.getRotation());
        // 直接拷贝协方差
        p_cov.covariance = gps_odom.pose.covariance;
        odom_out.pose = p_cov;

        // 6. 线速度和角速度只旋转方向
        tf2::Vector3 lin_in(
            gps_odom.twist.twist.linear.x,
            gps_odom.twist.twist.linear.y,
            gps_odom.twist.twist.linear.z);
        tf2::Vector3 ang_in(
            gps_odom.twist.twist.angular.x,
            gps_odom.twist.twist.angular.y,
            gps_odom.twist.twist.angular.z);

        // 旋转速度向量
        tf2::Vector3 lin_out = tf2_xform.getBasis() * lin_in;
        tf2::Vector3 ang_out = tf2_xform.getBasis() * ang_in;

        odom_out.twist.twist.linear.x = lin_out.x();
        odom_out.twist.twist.linear.y = lin_out.y();
        odom_out.twist.twist.linear.z = lin_out.z();
        odom_out.twist.twist.angular.x = ang_out.x();
        odom_out.twist.twist.angular.y = ang_out.y();
        odom_out.twist.twist.angular.z = ang_out.z();

        // 其它字段可根据需要进行补充
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<CalibOdomTransformer>();

    // 使用多线程执行器
    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node);
    executor.spin();

    rclcpp::shutdown();
    return 0;
}
